package estrategia;

import java.io.IOException;

import javax.vecmath.Point3d;

import monitor.Constantes;
import monitor.Salird;

import comunicacion.Conector;
import comunicacion.simulador.Parametros;
import comunicacion.simulador.Simulador;



public class Controlador {
	public static int MI_ROBOT;
	public static final double DISTANCIA_ENTRE_RUEDAS = 0.12;
	
	public static final int CONECTAR_CON_SIMULADOR = 1;
	public static final int CONECTAR_CON_ENTORNO_REAL = 2;
		
	public static final String comandosIP = "192.168.32.29"; 
	public static final int comandosPuerto = 1234;
	public static final String macRobot = "00:16:53:08:6A:F4";
	
	public static final String simIP = "127.0.0.1";
	//public static final String simIP = "192.168.10.101";
	public static final int simPuerto = 34465;
	
	public Conector entorno;
	private Parametros parametros;
	public Predictor predictor;
	
	private Utiles utiles;
	private Salird salird;
	
	
	public Controlador(int entornoConexion, int idRobot) throws IOException {
		switch (entornoConexion) {
			case CONECTAR_CON_SIMULADOR:
				MI_ROBOT = idRobot;
				entorno = new Simulador(simIP, simPuerto, idRobot, 0, 1);
				break;
			default:
				entorno = null;	
		}
		
		parametros = entorno.getParametros();
		utiles = new Utiles(entorno);
		predictor = new Predictor(entorno, MI_ROBOT);
				
		Constantes.cargarObjetos();
		
		salird = new Salird(this);
		salird.start();		
	}
	
	public void moverRobotLyapunov(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		while (distancia > parametros.DISTANCIA_ACEPTABLE) {			
			double v = utiles.velocidadLinealLV(actual, destino);				
			double w = utiles.velocidadAngularLV(actual, destino);
			
			double vl = v - w*DISTANCIA_ENTRE_RUEDAS/2.0;
			double vr = v + w*DISTANCIA_ENTRE_RUEDAS/2.0;
			
			entorno.enviarVelocidades(vl, vr);
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
			
		}
		
		entorno.detenerMotores();
	}
	
	public void moverRobotDirect(Point3d destino) throws IOException {
		
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		double maxVelocidad = parametros.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = parametros.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > parametros.DISTANCIA_ACEPTABLE) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
		}
		
		entorno.detenerMotores();
	}
	
	public void moverRobotInterceptarEnemigo(int idEnemigo) throws IOException {
		
		Point3d actual = entorno.getCoordenadas();
		Point3d destino = entorno.getCoordenadas(idEnemigo);		
		double distancia = utiles.distanciaEuclidea(actual, destino);		
		double RELACION_DISTANCIA_PASOS_HACIA_FUTURO = 40;
		
		double maxVelocidad = parametros.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = parametros.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > 2*parametros.DISTANCIA_ACEPTABLE) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			int pasosHaciaFuturo = (int) (distancia * RELACION_DISTANCIA_PASOS_HACIA_FUTURO);
			destino = predictor.predecirPosicion(idEnemigo, pasosHaciaFuturo);
			distancia = utiles.distanciaEuclidea(actual, destino);		
		}
		
		entorno.detenerMotores();
		System.out.println("Te atrape elemento " + idEnemigo);
	}
	
	public void moverRobotMixto(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas();
		int cuadrante = utiles.getCuadrante(actual, destino);
		
		double DISTANCIA_LYAPUNOV_DIRECT = 0.5;
		
		switch (cuadrante) {
			case 1: 
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT && 
						1.79 - destino.y > DISTANCIA_LYAPUNOV_DIRECT) {						
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y + margen+0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 4:
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT &&						
						destino.y > DISTANCIA_LYAPUNOV_DIRECT) {
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y - margen-0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 2: case 3:
				moverRobotLyapunov(destino);
		}
	}	
	
	public Point3d[] getPosicionesObstaculos() throws IOException {
		
		return entorno.getCoordenadasObjetos();
	}
	
	public Point3d getMiPosicion() throws IOException {
		return entorno.getCoordenadas();
	}
	
	
	
	public void detenerMotores() throws IOException {
		
		entorno.detenerMotores();
	}
	
	public void girar(Point3d destino) {
		try {
			Point3d actual = entorno.getCoordenadas();
			double theta = utiles.getAnguloTheta(actual, destino);
			
			girar(theta);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	
	
	public void girar(double theta) {
		System.out.println("Angulo1 = " + theta);
		
		if (theta > 2*Math.PI) {
			theta = theta % 2*Math.PI;
		}
		System.out.println("Angulo1 = " + theta);
		
		if (theta > Math.PI) {
			theta = theta - 2*Math.PI;
		}
		
		System.out.println("Angulo1 = " + theta);
		
		try {
			Point3d actual = entorno.getCoordenadas();
		
			double diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
			
			System.out.println("Diferencia = " + diferencia);
			
			int izq = 1;
			if (theta >=0 && actual.z >=0 || theta <=0 && actual.z <=0) {
				if (theta <= actual.z) {
					izq = 1;
				} else {
					izq = -1;
				}
			} else {
				if (actual.z <= 0 && theta >= 0) {
					if (actual.z + Math.PI >= theta) {
						izq = -1;
					} else {
						izq = 1;
					}
				} else {
					if (actual.z - Math.PI <= theta) {
						izq = 1;
					} else {
						izq = -1;
					}
				}
			}
			
			//while ( diferencia > conector.getToleranciaAngulo()) {
			while(!Salird.angulosIguales(theta, actual.z)) {
				entorno.enviarVelocidades(izq*(Math.sqrt(diferencia)), -izq*(Math.sqrt(diferencia)));
				actual = entorno.getCoordenadas();
				//diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
				//diferencia = Salird.angulosIguales(theta, actual.z);
				//System.out.println("Diferencia78 = " + diferencia);
			}
			
			entorno.detenerMotores();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	public void retroceder(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(-velocidad, -velocidad);	
		
	}
	
	public void avanzar(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(velocidad, velocidad);	
		
	}
	
	public void camposDePotencial(Point3d objetivo) throws IOException {
		
		CamposPotencial camposPotencial = new CamposPotencial();
		Point3d mipos = entorno.getCoordenadas();
		Point3d proxPunto = mipos;
		
		while (utiles.distanciaEuclidea(proxPunto, objetivo) > parametros.DISTANCIA_ACEPTABLE) {
			mipos = entorno.getCoordenadas();
			Point3d[] obstaculos = getPosicionesObstaculos();
			proxPunto = camposPotencial.hallarProximoDestino(mipos, objetivo, obstaculos);
			
			girar(proxPunto);
			moverRobotDirect(proxPunto);		
		}	
	}	
	
	
	public void camposPotencialUnivectorial(Point3d g, Point3d r) throws IOException {
			CamposPotencialUnivectorial campos = new CamposPotencialUnivectorial(utiles);
			
			Point3d p = entorno.getCoordenadas();
			
			double Kp = 20;
			double Kd = 40;
			
			double theta_error_anterior = 0;
			double distancia = utiles.distanciaEuclidea(p, g);
			
			while (distancia > parametros.DISTANCIA_ACEPTABLE) {
			
				Point3d[] obstaculos = entorno.getCoordenadasObjetos();
				Vector resultante = campos.hallarDireccionCampo(p, g, r, obstaculos);
				
				double theta_error = resultante.direccion - p.z; 
				
				double theta_error_der = theta_error - theta_error_anterior;
				theta_error_anterior = theta_error;
				
				double w = Kp*theta_error + Kd*theta_error_der;
				
				double v = utiles.velocidadLinealLV(p, g);
				double Vr = v + (DISTANCIA_ENTRE_RUEDAS/2)*w;
				double Vl = v - (DISTANCIA_ENTRE_RUEDAS/2)*w;
				
				entorno.enviarVelocidades(Vl, Vr);
				
				p = entorno.getCoordenadas();
				distancia = utiles.distanciaEuclidea(p, g);
				
			}
			entorno.detenerMotores();
		}
}